// ImmKpSerialTracker.cpp: implementation of the ImmKpSerialTracker class.
//
//////////////////////////////////////////////////////////////////////

#include "ImmKpSerialTracker.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

ImmKpSerialTracker::ImmKpSerialTracker()
{
	bTrackerPresent = false;
}

ImmKpSerialTracker::~ImmKpSerialTracker()
{

}

BOOL ImmKpSerialTracker::SERIAL_openTracker(char *cPort)
{
	unsigned char byte, string[50];
	BOOL timeoutError;
        unsigned short cnt;
	short deviceType, firmwareVersion;


	// Open serial port at 1200 baud
	if( ! SERIAL_initPort( cPort ) )
	{
		return FALSE;
	}


	// Toggle RTS line

	CommParam.fRtsControl = RTS_CONTROL_DISABLE;
	SetCommState( hPort, &CommParam );

	// Normally, a 100ms delay should be left between resetting
	// the RTS line to low, and setting it to high again. However,
	// the delay seems to upset Windows NT and it ends up
	// toggling the RTS line twice behind the scenes. If anyone
	// knows why it does this, I'd be interested to know!
//	  delay( 0.1F );  // 100 milliseconds

	CommParam.fRtsControl = RTS_CONTROL_ENABLE;			
	SetCommState (hPort, &CommParam);			


	// There could be characters left over in the serial buffer
	// from before the reset. Make sure these are removed.
	SERIAL_flushReadBuffer();



        // Switch to command mode
        SERIAL_out( 'c' );

	// The tracker will now send back the 'o' character acknowledgement.
        timeoutError = SERIAL_in( &byte );
        if( ( timeoutError == -1 ) || ( byte != 'o' ) )
	{
		printf( "Tracker not present on %s.\n", cPort );
		SERIAL_restore();
                return FALSE;
	}



        // Read and display version information
	// Note: this is optional, and is not a requirement
	// for initialising the tracker
        SERIAL_out( 'v' );
        SERIAL_out( 0 );  // Device no. must be 0

        timeoutError = SERIAL_in( &byte );
        if( ( timeoutError == -1 ) || ( byte != 'v' ) )  return FALSE;
        timeoutError = SERIAL_in( &byte );
        if( ( timeoutError == -1 ) || ( byte != 0 ) )  return FALSE;

        // Read device type
        if( SERIAL_readShort( &deviceType ) == -1 )  return FALSE;

        if( deviceType == 0 )
        {
                printf( "\nFound headset tracker on %s.\n", cPort );
        }

        // Read firmware version
        if( SERIAL_readShort( &firmwareVersion ) == -1 )  return FALSE;

        printf( "Firmware version: v%g\n", (float)firmwareVersion / 100 );

        // Read product string
        for( cnt = 0; cnt < 50; cnt++ )
        {
                if( SERIAL_in( &string[cnt] ) == -1 )  return FALSE;
        }

        printf( "%s\n\n", string );


        // Read and display number of devices present (optional)
	// Note: this is optional, and is not a requirement
	// for initialising the tracker
        SERIAL_out( 'p' );
        timeoutError = SERIAL_in( &byte );
        if( ( timeoutError == -1 ) || ( byte != 'p' ) )  return FALSE;
        if( SERIAL_in( &byte ) == -1 )  return FALSE;
        printf( "Devices present:  %d\n", (unsigned short)byte );



        // Finished setting up tracker, so switch to data mode
        SERIAL_out( 'd' );

        timeoutError = SERIAL_in( &byte );
        if( ( timeoutError == -1 ) || ( byte != 'd' ) )  return FALSE;


	// Set baud rate to 38400
	GetCommState( hPort, &CommParam );
	CommParam.BaudRate = CBR_38400;
	SetCommState( hPort, &CommParam );

	return TRUE;
}

void ImmKpSerialTracker::SERIAL_read3DOF(float *azAngle, float *elAngle, float *twAngle)
{
        struct INT_ANGLES_3DOF {
	        short azIntAngle, elIntAngle, twIntAngle;
        };
        static union {
                unsigned char bytes[6];
                struct INT_ANGLES_3DOF angles;
        } angleBuffer;

	static float previousAz = 0.0F, previousEl = 0.0F, previousTw = 0.0F;

	DWORD charsRead, totalCharsRead;
	long startTime;


	// First flush any characters in the serial buffer
	SERIAL_flushReadBuffer();


	// Request a 3DOF data packet from the tracker
	SERIAL_out( '3' );

	
	// You could do some processing here while waiting
	// for the tracker data packet to be sent. It takes
	// 1.5ms for a 3DOF data packet to be sent and 1ms
	// for a 2DOF data packet.
	Sleep(2);

        startTime = clock();

	// Read the 3 angles (2 bytes per angle = 6 bytes)
	ReadFile( hPort, (LPVOID)angleBuffer.bytes, 6, &totalCharsRead, NULL );

	while( totalCharsRead < 6 )
	{
		// If less than 6 bytes have been read, try
		// reading the remaining bytes
		ReadFile( hPort, (LPVOID)&angleBuffer.bytes[totalCharsRead], 6 - totalCharsRead, &charsRead, NULL );
		totalCharsRead += charsRead;

		// If we've timed out (5ms), then return the last read
		// angle position. The timeout is measured
		// using the clock() function, but it should be possible
		// to use the built in ReadFile timeouts to do this (see
		// SetCommTimeouts Windows function)
                if( ( clock() - startTime ) > 5 )
                {
			*azAngle = previousAz;
			*elAngle = previousEl;
			*twAngle = previousTw;
			return;
		}

	}

	// Return angles
	*azAngle = ( (float)angleBuffer.angles.azIntAngle * 180 ) / 32768;
	*elAngle = ( (float)angleBuffer.angles.elIntAngle * 180 ) / 32768;
	*twAngle = ( (float)angleBuffer.angles.twIntAngle * 180 ) / 32768;

	previousAz = *azAngle;
	previousEl = *elAngle;
	previousTw = *twAngle;
}

BOOL ImmKpSerialTracker::SERIAL_initPort(char *cPort)
{
	COMMTIMEOUTS CommTimeout; 

	// Open serial port
	hPort = CreateFile( cPort, GENERIC_READ | GENERIC_WRITE, 0,
			    NULL, OPEN_EXISTING,
			    FILE_ATTRIBUTE_NORMAL,
			    NULL );         

	if( hPort == INVALID_HANDLE_VALUE )
	{
		LPTSTR lpMsgBuf;
 
		FormatMessage(
			FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM,
			NULL,
			GetLastError(),
			MAKELANGID( LANG_NEUTRAL, SUBLANG_DEFAULT ), // Default language
			(LPTSTR) &lpMsgBuf,
			0,
			NULL
		);

		printf( "Unable to open %s - %s", cPort, lpMsgBuf );

		// Free the buffer.
		LocalFree( lpMsgBuf );

		return FALSE;
	}


	// Set timeouts - the settings specify that the Windows
	// function ReadFile will return immediately however many
	// characters have been successfully read.
	GetCommTimeouts( hPort, &CommTimeout );
	CommTimeout.ReadIntervalTimeout = MAXDWORD;
	CommTimeout.ReadTotalTimeoutMultiplier = 0;
	CommTimeout.ReadTotalTimeoutConstant = 0;
	CommTimeout.WriteTotalTimeoutMultiplier = 0;
	CommTimeout.WriteTotalTimeoutConstant = 5000;
	SetCommTimeouts( hPort, &CommTimeout ); 

	// Set serial parameters. Baud rate = 1200
	GetCommState( hPort, &CommParam );
	CommParam.BaudRate = CBR_1200;
	CommParam.ByteSize = 8;
	CommParam.Parity = NOPARITY;
	CommParam.StopBits = ONESTOPBIT;
	CommParam.fOutX = FALSE;
	CommParam.fInX = FALSE;
	CommParam.fRtsControl = RTS_CONTROL_DISABLE;
	SetCommState( hPort, &CommParam );


	printf( "Opened port %s.\n", cPort );

	return TRUE;
}

void ImmKpSerialTracker::SERIAL_restore()
{
	CloseHandle( hPort );
}

BOOL ImmKpSerialTracker::SERIAL_in(unsigned char *ch)  /* Get byte from port:If error return value -1 */
{
	DWORD charsRead;
        long startTime;

        startTime = clock();

	do {
		ReadFile( hPort, (LPVOID)ch, 1, &charsRead, NULL );

                if( ( clock() - startTime ) > ( CLOCKS_PER_SEC / 2 ) )
                {
                        // half second time out
                        return -1;
                }
	} while( charsRead < 1 );

        return( 0 );
}

BOOL ImmKpSerialTracker::SERIAL_readShort(short *num)
{
        union {
                unsigned char bytes[2];
                short word;
        } bytesToWord;

        if( SERIAL_in( &bytesToWord.bytes[0] ) == -1 )  return -1;
        if( SERIAL_in( &bytesToWord.bytes[1] ) == -1 )  return -1;

	*num = bytesToWord.word;

	return 0;
}

void ImmKpSerialTracker::SERIAL_out(unsigned char ch) /* Get byte from port:If error return value -1 */
{
	DWORD charsWritten;

	WriteFile( hPort, (LPCVOID)&ch, 1, &charsWritten, NULL );
}

void ImmKpSerialTracker::SERIAL_flushReadBuffer()
{
	PurgeComm( hPort, PURGE_RXCLEAR );
}

int ImmKpSerialTracker::Create()
{
	bTrackerPresent = true;
	com = 1;
	if( !SERIAL_openTracker( "COM1" ) )
	{
		com = 2;
		if( !SERIAL_openTracker( "COM2" ) )
		{
			com = 3;
			if( !SERIAL_openTracker( "COM3" ) )
			{
				com = 4;
				if( !SERIAL_openTracker( "COM4" ) )
				{
					bTrackerPresent = false;
				}
			}
		}
	}
	return bTrackerPresent;	
}
